/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation for Unit interface client library.
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#include "UnitClient.hpp"
#include "UnitData.hpp"

//*****************************************************************************
using namespace MaCI::Unit;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CUnitClient::CUnitClient(gimi::GIMI *aGIMIPtr, 
                           const int aInterfaceMinor)
  : CSync(1,1), // Max One Mutex, Max one Condition
    CThread(1), // Max one Thread.
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_UNIT, aInterfaceMinor),
    iUnitDataSequence(0),
    iLastUnitData(),
    iUnitClientCallback(NULL),
    iAvailableUnitArray()
{
  dPrint(4,"CUnitClient %p constructed", this);
}
//*****************************************************************************

CUnitClient::~CUnitClient()
{
  DoClose();
  dPrint(4,"CUnitClient %p destructed", this);
}
//*****************************************************************************

MaCI::EMaCIError CUnitClient::DoOpen(void)
{

  EMaCIError result = KMaCIError;
  
  // Checks.
  if(!IsThreadRunning()) {

    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();

    // If all inits were fine...
    if (result == KMaCIOK) {
      // Start the Thread
      SetActive(true);
      EnableSynchronousMessageReply();


      CThread::RunThread();
      dPrint(4,"CUnitClient::DoOpen() was succesfull.");
    

      // Do initial Service probe
      TAvailableUnitArray x;
      if (!GetAvailableUnits(x, TimeoutValue(1000))) {
        dPrint(ODERROR,"Timed out executing Initial GetAvailableUnits call with Timeout %u ms",
               TimeoutValue(1000));
        result = KMaCITimeout;
      }      
    }

  } else {
    dPrint(4,"UnitClient already Active, not Opening again.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CUnitClient::DoClose(void)
{
  EMaCIError result = KMaCIError;

  if(IsThreadRunning()) {
    SetActive(false);

    // Unsub
    result = UnsubscribeToInterface(TimeoutValue(2000));
  

    // Wait for thread to complete.
    CThread::SetRequestTermination();

    result = UnsubscribeToInterface(TimeoutValue(2000));
 

    result = UnsubscribeToInterface(TimeoutValue(2000));
 
    CThread::WaitThread();

    if(result == KMaCIOK){
      dPrint(4,"Succesfully closed UnitClient");    
    }else{
      dPrint(4,"Unsubscribing failed!");
    }
  
    
  } else {
    dPrint(4, "UnitClient not active, so not Closing.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************

bool CUnitClient::GetAvailableUnits(TAvailableUnitArray &aAvailableUnits, const unsigned int &aTimeout_ms)
{
  bool result = false;
  if (aTimeout_ms) {
    aAvailableUnits.clear();

    // Do direct Query
    const unsigned int umid = GetUniqueMessageID();

    // Send command
    CUnitData reply;
    CUnitData ud;
    ud.CreateInternalBinBag();
    ud.SetTimestamp(Common::TTimestamp());
    ud.SetCommand(TCommand(KCommandGetAvailableUnits));
    result = SendCommand(ud, umid);

    // SendCommand OK?
    if (result) {

      // Get Command Reply
      result = GetCommandReply(reply, umid, aTimeout_ms);
     
      // GetCommandReply OK?
      if (result) {
        const TAvailableUnitPtrArray aupa = reply.GetAvailableUnitArray();

        for(EACH_IN_i(aupa)) {
          const TAvailableUnit *au = *i;
          assert(au != NULL);

          // This works; as we have a conversion from TAvailableType
          // to SAvailableType (Naming is unfortunate :/ )
          aAvailableUnits.push_back(*au);
        }

        // Last but not least; Store the last received array to local storage
        Lock();
        iAvailableUnitArray = aAvailableUnits;
        Unlock();

      }
    }

  } else {
    // Local copy.
    Lock();
    aAvailableUnits = iAvailableUnitArray;
    Unlock();
  }


  return result;
}
//*****************************************************************************

bool CUnitClient::CreateUnit(TActiveUnit &aInfo, 
                             const uint32_t &aAvailableUnitId, 
                             const std::string &aName,
                             const TUnitPose &aUnitPose,
                             const unsigned int &aTimeout_ms)
{  
  bool result = false;
  
  // Do direct Query
  const unsigned int umid = GetUniqueMessageID();
  
  // Send command
  CUnitData reply;
  CUnitData ud;
  ud.CreateInternalBinBag();
  ud.SetTimestamp(Common::TTimestamp());
  ud.SetCommand(TCommand(KCommandCreateUnit));
  ud.SetUnitId(TUnitId(aAvailableUnitId, 0, aName));
  ud.SetUnitPose(aUnitPose);
  
  result = SendCommand(ud, umid);
  
  // SendCommand OK?
  if (result) {
    
    // Get Command Reply
    result = GetCommandReply(reply, umid, aTimeout_ms);
    
    // GetCommandReply OK?
    if (result) {
      const TCommand *cmd = reply.GetCommand();
      const TUnitId *unitid = reply.GetUnitId();
      if (cmd && unitid) {
        if (cmd->flags & (KCommandFlagReply|KCommandFlagResultOK)) {
          aInfo = *unitid;
        } else result = false;
      } else result = false;
      
    }
  }
  return result;
}
//*****************************************************************************

bool CUnitClient::DeleteUnit(const uint32_t &aActiveUnitId,
                             const unsigned int &aTimeout_ms)
{  
  bool result = false;
  
  // Do direct Query
  const unsigned int umid = GetUniqueMessageID();
  
  // Send command
  CUnitData reply;
  CUnitData ud;
  ud.CreateInternalBinBag();
  ud.SetTimestamp(Common::TTimestamp());
  ud.SetCommand(TCommand(KCommandDeleteUnit));
  ud.SetUnitId(TUnitId(0, aActiveUnitId, ""));
  
  result = SendCommand(ud, umid);
  
  // SendCommand OK?
  if (result) {
    
    // Get Command Reply
    result = GetCommandReply(reply, umid, aTimeout_ms);
    
    // GetCommandReply OK?
    if (result) {
      const TCommand *cmd = reply.GetCommand();
      const TUnitId *unitid = reply.GetUnitId();
      if (cmd && unitid) {
        if (cmd->flags & (KCommandFlagReply|KCommandFlagResultOK)) {

        } else result = false;
      } else result = false;
      
    }
  }
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CUnitClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CUnitData id;
  return result;
}

//*****************************************************************************

int CUnitClient::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  /*  const unsigned int hard_error_count_limit = 10;
      unsigned int hard_error_count = 0;*/
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(8, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(IsActive()) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000);
    if (e == KMaCIOK) {
      ProcessMessage(gmsg);

      // Reset error counter.
      // if (hard_error_count) hard_error_count = 0;

    } else if (e != KMaCITimeout) {

      if(IsActive()){
        // Other error, try to recover by sleeping one second.
        dPrint(3,"ProcessMessage returned error %u and client still active. Trying to recover by sleeping 1000ms ",
               e);
        ownSleep_ms(1000);
      }else{
        //
        dPrint(3,"ProcessMessage returned error %u. Normal situtation because client is already closed",
               e);
      }
  
      /*
        
      // Increment hard error count
      ++hard_error_count;
      
      // Check whether the hard error count limit is still under limit. 
      if (hard_error_count < hard_error_count_limit) {
        // Other error, try to recover by sleeping one second.
        dPrint(10,"ProcessMessage returned error %u, trying to recover by sleeping 1000ms (%u attempts left)",
               e, hard_error_count_limit-hard_error_count);
        ownSleep_ms(1000);

      } else {
        dPrint(3,"Error Recovery failed, UnitClient ThreadFunction terminating...");

        // Break the while().
        break;
      }
      */
    }
  }
  
  dPrint(8, "GIMIMessage handler stopped");
  SetActive(false);

  return result;
}
//*****************************************************************************

bool CUnitClient::SendCommand(const CUnitData &aUnitData, const int aMessageID)
{
  EMaCIError result = KMaCIError;
  
  if (IsActive()) {
    if (aUnitData.GetCommand()) {
      // Send.
      SendInterfaceMessage(aUnitData.GetBinBagContainerPtr()->GetBuffer(),
                           aUnitData.GetBinBagContainerPtr()->GetBufferSize(),
                           aMessageID);
      
      dPrint(10,"SendInterfaceMessage called: %d:%d (%d bytes of payload sent)",
             iInterfaceMajor, iInterfaceMinor, 
             aUnitData.GetBinBagContainerPtr()->GetBufferSize());
      // Result
      result = KMaCIOK;
      
    } else {
      dPrint(2,"No 'TCommand' element in provided CUnitData object. Will not send!");
      result = KMaCIInvalidFormat;

    }
   
  } else {
    dPrint(2,"Unable to send UnitData - Interface is not active! (Open() not called?)");
    result = KMaCINotActive;

  }


  return (result == KMaCIOK);
}
//*****************************************************************************

bool CUnitClient::GetCommandReply(CUnitData &aUnitData, const int aMessageID, 
                                  const int aTimeout_ms)
{ 
  EMaCIError result;
  SMessageSource ms; // Compulsory for call to
                     // Receivesynchronousmessagereply, not used here
                     // though.

  // Call the ReceiveMessage wrapper with 2s timeout
  result = ReceiveSynchronousMessageReply(aUnitData, ms, aMessageID, aTimeout_ms);
  
  // If result is OK, process the msg.
  if (result == KMaCIOK) {
    // Got message OK.  Basicly we don't need to do anything here, but
    // some interface may want to do some checks: for example, the
    // received message contains some specific element or information
    // before it is accepted.
  }
  
  return (result == KMaCIOK);
}
//*****************************************************************************
//*****************************************************************************
